function [state, P] = ekf_predict(state, P, control, dt, Q)
% EKF预测步骤
% 输入: state-状态向量, P-协方差矩阵, control-控制量[v;w], dt-时间步长, Q-过程噪声
% 输出: state-预测状态, P-预测协方差

    x = state(1);
    y = state(2);
    theta = state(3);
    v = control(1);
    omega = control(2);
    
    % 运动模型
    if abs(omega) < 1e-6
        x_pred = x + v * dt * cos(theta);
        y_pred = y + v * dt * sin(theta);
        theta_pred = theta;
    else
        x_pred = x + (v/omega) * (sin(theta + omega*dt) - sin(theta));
        y_pred = y + (v/omega) * (-cos(theta + omega*dt) + cos(theta));
        theta_pred = theta + omega * dt;
    end
    
    theta_pred = wrapToPi(theta_pred);
    state(1:3) = [x_pred; y_pred; theta_pred];
    
    % 雅可比矩阵（正确的非线性化）
    n = length(state);
    
    if abs(omega) < 1e-6
        % 直线运动
        Fx = [1, 0, -v*dt*sin(theta);
              0, 1,  v*dt*cos(theta);
              0, 0,  1];
    else
        % 圆弧运动
        Fx = [1, 0, (v/omega)*(cos(theta + omega*dt) - cos(theta));
              0, 1, (v/omega)*(sin(theta + omega*dt) - sin(theta));
              0, 0, 1];
    end
    
    F = eye(n);
    F(1:3, 1:3) = Fx;
    
    W = zeros(n, 3);
    W(1:3, 1:3) = eye(3);
    
    P = F * P * F' + W * Q * W';
    P = (P + P') / 2;
    P = P + eye(n) * 1e-9;
end

